#include "BezierCurveEvaluator.h"
#include <assert.h>

void BezierCurveEvaluator::evaluateCurve(const std::vector<Point>& ptvCtrlPts, 
										 std::vector<Point>& ptvEvaluatedCurvePts, 
										 const float& fAniLength, 
										 const bool& bWrap) const
{
	
	ptvEvaluatedCurvePts.clear();

	// interpolating points are 0, 3, 6...
	if (!bWrap || ptvCtrlPts.size() % 3 != 0) {

		// Handle first point
		if (!bWrap) {
			// Add in first point at the start of the animation
			ptvEvaluatedCurvePts.push_back(Point(0, ptvCtrlPts[0].y)); 
		} else {
			// interpolate to find "fake" starting point
			ptvEvaluatedCurvePts.push_back(Point(0, ptvCtrlPts.back().y + (ptvCtrlPts.front().y - ptvCtrlPts.back().y) / (fAniLength - ptvCtrlPts.back().x + ptvCtrlPts.front().x) * (fAniLength - ptvCtrlPts.back().x))); 
		}

		// Main Bezier Curve (not end points)
		unsigned int end = 0;
		for (unsigned int x = 0; x + 3< ptvCtrlPts.size(); x+=3) {
			end = x+4;
			std::vector<Point> pt = getBezier(ptvCtrlPts[x], ptvCtrlPts[x+1], ptvCtrlPts[x+2], ptvCtrlPts[x+3]);

			for (int y = 0; y < pt.size(); y++) {
				ptvEvaluatedCurvePts.push_back(pt[y]);
			}
		}	

		// Leftovers are linearly interpolated (numpoints mod 3)
		for (; end < ptvCtrlPts.size(); end++) {
			ptvEvaluatedCurvePts.push_back(ptvCtrlPts[end]);
		}

		// Handle last point
		if (!bWrap) {
			// Add in first point at the start of the animation
			ptvEvaluatedCurvePts.push_back(Point(fAniLength, ptvCtrlPts.back().y)); 
		} else {
			// interpolate to find "fake" starting point
			ptvEvaluatedCurvePts.push_back(Point(fAniLength, ptvCtrlPts.back().y + (ptvCtrlPts.front().y - ptvCtrlPts.back().y) / (fAniLength - ptvCtrlPts.back().x + ptvCtrlPts.front().x) * (fAniLength - ptvCtrlPts.back().x))); 
		}

	} else { // 3k points and wrap

		vector<Point> temp;
		// Main Bezier Curve (not end points) //
		for (unsigned int x = 0; x + 3 < ptvCtrlPts.size(); x+=3) {
			std::vector<Point> pt = getBezier(ptvCtrlPts[x], ptvCtrlPts[x+1], ptvCtrlPts[x+2], ptvCtrlPts[x+3]);

			for (int y = 0; y < pt.size(); y++) {
				temp.push_back(pt[y]);
			}
		}

		// Wraparound segment //
		std::vector<Point> pt = getBezier(ptvCtrlPts[ptvCtrlPts.size() - 3],
										  ptvCtrlPts[ptvCtrlPts.size() - 2],
										  ptvCtrlPts[ptvCtrlPts.size() - 1],
										  Point(ptvCtrlPts[0].x + fAniLength, ptvCtrlPts[0].y));

		
		// Merge everything together
		for (std::vector<Point>::iterator i = pt.begin(); i != pt.end(); i++) {
			if (i->x >= fAniLength) ptvEvaluatedCurvePts.push_back(Point(i->x-fAniLength, i->y));
			else temp.push_back(*i);
		}

		for (std::vector<Point>::iterator i = temp.begin(); i != temp.end(); i++) {
			ptvEvaluatedCurvePts.push_back(*i);
		}
	}

	ptvEvaluatedCurvePts = trimPoints(ptvEvaluatedCurvePts);

}

std::vector<Point> BezierCurveEvaluator::getBezier(Point v0, Point v1, Point v2, Point v3) const {
	
	// basis matrix for bezier
	Mat4<double> basis(-1, 3, -3, 1, 3, -6, 3, 0, -3, 3, 0, 0, 1, 0, 0, 0);
	basis = basis.transpose();
	
	Vec4<double> ctrl_x(v0.x, v1.x, v2.x, v3.x);
	Vec4<double> ctrl_y(v0.y, v1.y, v2.y, v3.y);
	
	return evaluateSpline(basis, ctrl_x, ctrl_y, BEZIER_PARAMETER_SAMPLE);

}

bool BezierCurveEvaluator::flatEnough(Point v0, Point v1, Point v2, Point v3) {

	double sum_distances = v0.distance(v1) + v1.distance(v2) + v2.distance(v3);
	double end_distances = v3.distance(v0);
	double ratio = sum_distances/end_distances;

	return ratio < 1.0 + BEZIER_EPSILON_FLAT;

}

void BezierCurveEvaluator::subDivide(Point v0, Point v1, Point v2, Point v3, std::vector<Point>& r1, std::vector<Point>& r2) {

	r1.clear();
	r2.clear();

	r1.push_back(v0);
	// r1.push_back((v0 + v1)/2);
	
}

